/**
  ****************************RM Warrior 2023****************************
  * @file       gimbal_task.c/h
  * @brief      完成云台控制任务，由于云台使用陀螺仪解算出的角度，其范围在（-pi,pi）
  *             故而设置目标角度均为范围，存在许多对角度计算的函数。云台主要分为2种
  *             状态，陀螺仪控制状态是利用板载陀螺仪解算的姿态角进行控制，编码器控制
  *             状态是通过电机反馈的编码值控制的校准，此外还有校准状态，停止状态等。
  * @note       
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     2023/2/         pxx              ......
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************RM Warrior 2023****************************
  */

#include "Gimbal_Task.h"

#include "main.h"

#include "arm_math.h"
#include "user_lib.h"
// #include "INS_Task.h"
#include "remote_control.h"
#include "pid.h"

#include "FreeRTOSConfig.h"
#include "FreeRTOS.h"
#include "task.h"
#include "gimbal_behavior.h"

//电机编码值规整 0—8191
#define ECD_Format(ecd)         \
    {                           \
        if ((ecd) > ecd_range)  \
            (ecd) -= ecd_range; \
        else if ((ecd) < 0)     \
            (ecd) += ecd_range; \
    }

#define gimbal_total_pid_clear(gimbal_clear)                                                   \
    {                                                                                          \
        PID_clear(&(gimbal_clear)->gimbal_yaw_motor.gimbal_motor_pixel_pid);                    \
                                                                                               \
        PID_clear(&(gimbal_clear)->gimbal_pitch_motor.gimbal_motor_pixel_pid);                  \
    }

#if INCLUDE_uxTaskGetStackHighWaterMark
uint32_t gimbal_high_water;
#endif

//云台控制所有相关数据
static Gimbal_Control_t gimbal_control;

//发送的can 指令
static int16_t Yaw_Can_Set_Current = 0, Pitch_Can_Set_Current = 0, Shoot_Can_Set_Current = 0;

//云台初始化
static void GIMBAL_Init(Gimbal_Control_t *gimbal_init);
//云台pid清零
static void Gimbal_PID_clear(Gimbal_PID_t *gimbal_pid_clear);
//云台状态设置
static void GIMBAL_Set_Mode(Gimbal_Control_t *gimbal_set_mode);
//云台数据更新
static void GIMBAL_Feedback_Update(Gimbal_Control_t *gimbal_feedback_update);
//云台状态切换保存数据，例如从陀螺仪状态切换到编码器状态保存目标值
static void GIMBAL_Mode_Change_Control_Transit(Gimbal_Control_t *gimbal_mode_change);
//计算云台电机相对中值的相对角度
static fp32 motor_ecd_to_angle_change(uint16_t ecd, uint16_t offset_ecd);
//设置云台控制量
static void GIMBAL_Set_Contorl(Gimbal_Control_t *gimbal_set_control);
//云台控制pid计算
static void GIMBAL_Control_loop(Gimbal_Control_t *gimbal_control_loop);

static void gimbal_motor_relative_angle_control(Gimbal_Motor_t *gimbal_motor);
static void gimbal_motor_raw_angle_control(Gimbal_Motor_t *gimbal_motor);
static void gimbal_motor_pixel_control(Gimbal_Motor_t *gimbal_motor);

//在陀螺仪角度控制下，对控制的目标值进限制以防超最大相对角度
static void GIMBAL_relative_angle_limit(Gimbal_Motor_t *gimbal_motor, fp32 add);
static void GIMBAL_PID_Init(Gimbal_PID_t *pid, fp32 maxout, fp32 intergral_limit, fp32 kp, fp32 ki, fp32 kd);
static fp32 GIMBAL_PID_Calc(Gimbal_PID_t *pid, fp32 get, fp32 set, fp32 error_delta);

void GIMBAL_task(void *pvParameters)
{
    //等待陀螺仪任务更新陀螺仪数据
    vTaskDelay(GIMBAL_TASK_INIT_TIME);
    //云台初始化
    gimbal_offset_init();
    GIMBAL_Init(&gimbal_control);
    
    while (1)
    {
        GIMBAL_Set_Mode(&gimbal_control);                    //设置云台控制模式
//        GIMBAL_Mode_Change_Control_Transit(&gimbal_control); //控制模式切换 控制数据过渡
//        GIMBAL_Feedback_Update(&gimbal_control);             //云台数据反馈
        GIMBAL_Set_Contorl(&gimbal_control);                 //设置云台控制量
        GIMBAL_Control_loop(&gimbal_control);                //云台控制PID计算
//        CAN_CMD_GIMBAL(Yaw_Can_Set_Current, Pitch_Can_Set_Current, Shoot_Can_Set_Current, 0);
//		printf("%d %d %d\n",gimbal_control.PI_Data->center_point.x, gimbal_control.PI_Data->point[2].x, gimbal_control.PI_Data->red_point.x);
//		printf("%f %f %f %f\n",gimbal_control.gimbal_pitch_motor.relative_angle*57.3,gimbal_control.gimbal_yaw_motor.relative_angle*57.3, gimbal_control.gimbal_pitch_motor.motor_gyro*57.3,gimbal_control.gimbal_yaw_motor.motor_gyro*57.3);
        vTaskDelay(GIMBAL_CONTROL_TIME);

#if INCLUDE_uxTaskGetStackHighWaterMark
        gimbal_high_water = uxTaskGetStackHighWaterMark(NULL);
#endif
    }
}


//初始化pid 数据指针
static void GIMBAL_Init(Gimbal_Control_t *gimbal_init)
{
//    static const fp32 Pitch_speed_pid[3] = {PITCH_SPEED_PID_KP, PITCH_SPEED_PID_KI, PITCH_SPEED_PID_KD};
//    static const fp32 Yaw_speed_pid[3] = {YAW_SPEED_PID_KP, YAW_SPEED_PID_KI, YAW_SPEED_PID_KD};
	static const fp32 Pitch_Pixel_pid[3] = {PITCH_PIXEL_PID_KP, PITCH_PIXEL_PID_KI, PITCH_PIXEL_PID_KD};
    static const fp32 Yaw_Pixel_pid[3] = {YAW_PIXEL_PID_KP, YAW_PIXEL_PID_KI, YAW_PIXEL_PID_KD};
	
    //电机数据指针获取
    gimbal_init->gimbal_yaw_motor.gimbal_motor_measure = get_yaw_data_point();
    gimbal_init->gimbal_pitch_motor.gimbal_motor_measure = get_pitch_data_point();
	
    //陀螺仪数据指针获取
//    gimbal_init->gimbal_INT_angle_point = get_INS_angle_point();
//    gimbal_init->gimbal_INT_gyro_point = get_gyro_data_point();
	//树莓派数据指针获取
	gimbal_init->PI_Data = get_pi_data_point();
	
	//控制量反馈
//	gimbal_init->pixel_control = get_pixel_control_data();
	//电机控制量反馈
//	gimbal_init->gimbal_pitch_motor.pixel_control = get_pitch_pixel_control_data();
//	gimbal_init->gimbal_yaw_motor.pixel_control = get_yaw_pixel_control_data();
	//状态获取

	
    //遥控器数据指针获取
    gimbal_init->gimbal_rc_ctrl = get_remote_control_point();
	
	//按键mode指向
	gimbal_init ->now_mode = get_mode_data_point();
	
    //初始化电机模式
    gimbal_init->gimbal_yaw_motor.gimbal_motor_mode = gimbal_init->gimbal_yaw_motor.last_gimbal_motor_mode = GIMBAL_MOTOR_RAW;
    gimbal_init->gimbal_pitch_motor.gimbal_motor_mode = gimbal_init->gimbal_pitch_motor.last_gimbal_motor_mode = GIMBAL_MOTOR_RAW;
	
    //初始化yaw电机pid
    PID_Init(&gimbal_init->gimbal_yaw_motor.gimbal_motor_pixel_pid, PID_POSITION,Yaw_Pixel_pid, YAW_PIXEL_PID_MAX_OUT, YAW_PIXEL_PID_MAX_IOUT);
//    PID_Init(&gimbal_init->gimbal_yaw_motor.gimbal_motor_gyro_pid, PID_POSITION, Yaw_speed_pid, YAW_SPEED_PID_MAX_OUT, YAW_SPEED_PID_MAX_IOUT);
    //初始化pitch电机pid
    PID_Init(&gimbal_init->gimbal_pitch_motor.gimbal_motor_pixel_pid,PID_POSITION,Pitch_Pixel_pid, PITCH_PIXEL_PID_MAX_OUT, PITCH_PIXEL_PID_MAX_IOUT);
//    PID_Init(&gimbal_init->gimbal_pitch_motor.gimbal_motor_gyro_pid, PID_POSITION, Pitch_speed_pid, PITCH_SPEED_PID_MAX_OUT, PITCH_SPEED_PID_MAX_IOUT);

    //清除所有PID
    gimbal_total_pid_clear(gimbal_init);

    GIMBAL_Feedback_Update(gimbal_init);

    gimbal_init->gimbal_yaw_motor.relative_angle_set = gimbal_init->gimbal_yaw_motor.relative_angle;
    gimbal_init->gimbal_yaw_motor.motor_gyro_set = gimbal_init->gimbal_yaw_motor.motor_gyro;


    gimbal_init->gimbal_pitch_motor.relative_angle_set = gimbal_init->gimbal_pitch_motor.relative_angle;
    gimbal_init->gimbal_pitch_motor.motor_gyro_set = gimbal_init->gimbal_pitch_motor.motor_gyro;
	
	
}

static void GIMBAL_Set_Mode(Gimbal_Control_t *gimbal_set_mode)
{
    if (gimbal_set_mode == NULL)
    {
        return;
    }
    gimbal_set_mode->gimbal_pitch_motor.gimbal_motor_mode = GIMBAL_MOTOR_ENCONDE;
    gimbal_set_mode->gimbal_yaw_motor.gimbal_motor_mode = GIMBAL_MOTOR_ENCONDE;
	gimbal_behaviour_set_mode(gimbal_set_mode);
}

static void GIMBAL_Feedback_Update(Gimbal_Control_t *gimbal_feedback_update)
{
    if (gimbal_feedback_update == NULL)
    {
        return;
    }
    //云台数据更新
    {//方案一
        //云台yaw电机加上了减速箱，因此要更改反馈量计算方式
//        static uint8_t time_count = 0;
        fp32 pitch_motor_angle = 0;
		fp32 yaw_motor_angle = 0;
		fp32 pitch_motor_gyro = 0;
		fp32 yaw_motor_gyro = 0;

		//电机转过的实际弧度为编码器值*比例系数
		pitch_motor_angle = gimbal_feedback_update->gimbal_pitch_motor.gimbal_motor_measure->ecd*Motor_Ecd_to_Rad;
		yaw_motor_angle   = gimbal_feedback_update->gimbal_yaw_motor.gimbal_motor_measure->ecd*Motor_Ecd_to_Rad;
		
		pitch_motor_gyro = (gimbal_feedback_update->gimbal_pitch_motor.gimbal_motor_measure->ecd - gimbal_feedback_update->gimbal_pitch_motor.gimbal_motor_measure->last_ecd)*Motor_Ecd_to_Rad*250;
		yaw_motor_gyro = (gimbal_feedback_update->gimbal_yaw_motor.gimbal_motor_measure->ecd - gimbal_feedback_update->gimbal_yaw_motor.gimbal_motor_measure->last_ecd)*Motor_Ecd_to_Rad*250;
		
		
		
		
		gimbal_feedback_update ->gimbal_pitch_motor.relative_angle = pitch_motor_angle*MOTOR_RAD_TO_REAL_RAD;
		gimbal_feedback_update ->gimbal_yaw_motor.relative_angle   = -yaw_motor_angle*MOTOR_RAD_TO_REAL_RAD;
		
		gimbal_feedback_update ->gimbal_pitch_motor.motor_gyro = pitch_motor_gyro*MOTOR_RAD_TO_REAL_RAD;
		gimbal_feedback_update ->gimbal_yaw_motor.motor_gyro   = -yaw_motor_gyro*MOTOR_RAD_TO_REAL_RAD;
		

    }
  
    

}
//计算相对角度
static fp32 motor_ecd_to_angle_change(uint16_t ecd, uint16_t offset_ecd)
{
    int32_t relative_ecd = ecd - offset_ecd;
    if (relative_ecd > Half_ecd_range)
    {
        relative_ecd -= ecd_range;
    }
    else if (relative_ecd < -Half_ecd_range)
    {
        relative_ecd += ecd_range;
    }

    return relative_ecd * Motor_Ecd_to_Rad;
}

//云台状态切换保存，用于状态切换过渡
static void GIMBAL_Mode_Change_Control_Transit(Gimbal_Control_t *gimbal_mode_change)
{
    if (gimbal_mode_change == NULL)
    {
        return;
    }
    //yaw电机状态机切换保存数据
    if (gimbal_mode_change->gimbal_yaw_motor.last_gimbal_motor_mode != GIMBAL_MOTOR_RAW && gimbal_mode_change->gimbal_yaw_motor.gimbal_motor_mode == GIMBAL_MOTOR_RAW)
    {
        gimbal_mode_change->gimbal_yaw_motor.raw_cmd_current = gimbal_mode_change->gimbal_yaw_motor.current_set = gimbal_mode_change->gimbal_yaw_motor.given_current;
    }
    else if (gimbal_mode_change->gimbal_yaw_motor.last_gimbal_motor_mode != GIMBAL_MOTOR_ENCONDE && gimbal_mode_change->gimbal_yaw_motor.gimbal_motor_mode == GIMBAL_MOTOR_ENCONDE)
    {
        gimbal_mode_change->gimbal_yaw_motor.relative_angle_set = gimbal_mode_change->gimbal_yaw_motor.relative_angle;
    }
    gimbal_mode_change->gimbal_yaw_motor.last_gimbal_motor_mode = gimbal_mode_change->gimbal_yaw_motor.gimbal_motor_mode;

    //pitch电机状态机切换保存数据
    if (gimbal_mode_change->gimbal_pitch_motor.last_gimbal_motor_mode != GIMBAL_MOTOR_RAW && gimbal_mode_change->gimbal_pitch_motor.gimbal_motor_mode == GIMBAL_MOTOR_RAW)
    {
        gimbal_mode_change->gimbal_pitch_motor.raw_cmd_current = gimbal_mode_change->gimbal_pitch_motor.current_set = gimbal_mode_change->gimbal_pitch_motor.given_current;
    }
    else if (gimbal_mode_change->gimbal_pitch_motor.last_gimbal_motor_mode != GIMBAL_MOTOR_ENCONDE && gimbal_mode_change->gimbal_pitch_motor.gimbal_motor_mode == GIMBAL_MOTOR_ENCONDE)
    {
        gimbal_mode_change->gimbal_pitch_motor.relative_angle_set = gimbal_mode_change->gimbal_pitch_motor.relative_angle;
    }

    gimbal_mode_change->gimbal_pitch_motor.last_gimbal_motor_mode = gimbal_mode_change->gimbal_pitch_motor.gimbal_motor_mode;
}

//云台控制量设置
static void GIMBAL_Set_Contorl(Gimbal_Control_t *gimbal_set_control)
{
    if (gimbal_set_control == NULL)
    {
        return;
    }

    fp32 add_yaw_angle = 0.0f;
    fp32 add_pitch_angle = 0.0f;

	add_yaw_angle = -GIMBAL_WZ_RC_SEN*(gimbal_set_control->gimbal_rc_ctrl->ch[0]);
	add_pitch_angle = -GIMBAL_WZ_RC_SEN*(gimbal_set_control->gimbal_rc_ctrl->ch[1]);
//	gimbal_set_control->gimbal_pitch_motor.relative_angle_set+=add_pitch_angle;
//	gimbal_set_control->gimbal_yaw_motor.relative_angle_set+=add_yaw_angle;
    gimbal_behaviour_control_set(&add_yaw_angle, &add_pitch_angle, gimbal_set_control);
    //yaw电机模式控制
    if (gimbal_set_control->gimbal_yaw_motor.gimbal_motor_mode == GIMBAL_MOTOR_RAW)
    {
        //raw模式下，直接发送控制值
        gimbal_set_control->gimbal_yaw_motor.raw_cmd_current = add_yaw_angle;
		if(gimbal_set_control->gimbal_yaw_motor.gimbal_motor_measure->real_one->relative_angle>=gimbal_set_control->gimbal_yaw_motor.max_relative_angle) {
			if(gimbal_set_control->gimbal_yaw_motor.raw_cmd_current > 0)
				gimbal_set_control->gimbal_yaw_motor.raw_cmd_current = 0;
		}
		
		else if(gimbal_set_control->gimbal_yaw_motor.gimbal_motor_measure->real_one->relative_angle<=gimbal_set_control->gimbal_yaw_motor.min_relative_angle) {
			 if(gimbal_set_control->gimbal_yaw_motor.raw_cmd_current < 0)
				gimbal_set_control->gimbal_yaw_motor.raw_cmd_current = 0;
		}
    }
    else if (gimbal_set_control->gimbal_yaw_motor.gimbal_motor_mode == GIMBAL_MOTOR_ENCONDE)
    {
        //enconde
//        GIMBAL_relative_angle_limit(&gimbal_set_control->gimbal_yaw_motor, add_yaw_angle);
		gimbal_set_control->gimbal_yaw_motor.pixel_cmd = add_yaw_angle;
		if(gimbal_set_control->gimbal_yaw_motor.gimbal_motor_measure->real_one->relative_angle>=gimbal_set_control->gimbal_yaw_motor.max_relative_angle) {
			if(gimbal_set_control->gimbal_yaw_motor.raw_cmd_current > 0)
				gimbal_set_control->gimbal_yaw_motor.raw_cmd_current = 0;
		}
		
		else if(gimbal_set_control->gimbal_yaw_motor.gimbal_motor_measure->real_one->relative_angle<=gimbal_set_control->gimbal_yaw_motor.min_relative_angle) {
			 if(gimbal_set_control->gimbal_yaw_motor.raw_cmd_current < 0)
				gimbal_set_control->gimbal_yaw_motor.raw_cmd_current = 0;
		}
    }

    //pitch电机模式控制
    if (gimbal_set_control->gimbal_pitch_motor.gimbal_motor_mode == GIMBAL_MOTOR_RAW)
    {
        //raw模式下，直接发送控制值
        gimbal_set_control->gimbal_pitch_motor.raw_cmd_current =add_pitch_angle;
		if(gimbal_set_control->gimbal_pitch_motor.gimbal_motor_measure->real_one->relative_angle>=gimbal_set_control->gimbal_pitch_motor.max_relative_angle) {
			if(gimbal_set_control->gimbal_pitch_motor.raw_cmd_current > 0)
				gimbal_set_control->gimbal_pitch_motor.raw_cmd_current = 0;
		}
		
		else if(gimbal_set_control->gimbal_pitch_motor.gimbal_motor_measure->real_one->relative_angle<=gimbal_set_control->gimbal_pitch_motor.min_relative_angle) {
			 if(gimbal_set_control->gimbal_pitch_motor.raw_cmd_current < 0)
				gimbal_set_control->gimbal_pitch_motor.raw_cmd_current = 0;
		}
    }
    else if (gimbal_set_control->gimbal_pitch_motor.gimbal_motor_mode == GIMBAL_MOTOR_ENCONDE)
    {
        //enconde模式下，电机编码角度控制
//        GIMBAL_relative_angle_limit(&gimbal_set_control->gimbal_pitch_motor, add_pitch_angle);
		gimbal_set_control->gimbal_pitch_motor.pixel_cmd = add_pitch_angle;
		if(gimbal_set_control->gimbal_pitch_motor.gimbal_motor_measure->real_one->relative_angle>=gimbal_set_control->gimbal_pitch_motor.max_relative_angle) {
			if(gimbal_set_control->gimbal_pitch_motor.raw_cmd_current > 0)
				gimbal_set_control->gimbal_pitch_motor.raw_cmd_current = 0;
		}
		
		else if(gimbal_set_control->gimbal_pitch_motor.gimbal_motor_measure->real_one->relative_angle<=gimbal_set_control->gimbal_pitch_motor.min_relative_angle) {
			 if(gimbal_set_control->gimbal_pitch_motor.raw_cmd_current < 0)
				gimbal_set_control->gimbal_pitch_motor.raw_cmd_current = 0;
		}
    }
}

static void GIMBAL_relative_angle_limit(Gimbal_Motor_t *gimbal_motor, fp32 add)
{
    if (gimbal_motor == NULL)
    {
        return;
    }
    gimbal_motor->relative_angle_set += add;
    //是否超过最大 最小值
    if (gimbal_motor->relative_angle_set > gimbal_motor->max_relative_angle)
    {
        gimbal_motor->relative_angle_set = gimbal_motor->max_relative_angle;
    }
    else if (gimbal_motor->relative_angle_set < gimbal_motor->min_relative_angle)
    {
        gimbal_motor->relative_angle_set = gimbal_motor->min_relative_angle;
    }

}
//云台控制状态使用不同控制pid
static void GIMBAL_Control_loop(Gimbal_Control_t *gimbal_control_loop)
{
    if (gimbal_control_loop == NULL)
    {
        return;
    }
    //yaw不同模式对于不同的控制函数
    if (gimbal_control_loop->gimbal_yaw_motor.gimbal_motor_mode == GIMBAL_MOTOR_RAW)
    {
        //raw控制
        gimbal_motor_raw_angle_control(&gimbal_control_loop->gimbal_yaw_motor);
    }
    else if (gimbal_control_loop->gimbal_yaw_motor.gimbal_motor_mode == GIMBAL_MOTOR_ENCONDE)
    {
        //enconde角度控制
//        gimbal_motor_relative_angle_control(&gimbal_control_loop->gimbal_yaw_motor);
		gimbal_motor_pixel_control(&gimbal_control_loop->gimbal_yaw_motor);
    }

    //pitch不同模式对于不同的控制函数
    if (gimbal_control_loop->gimbal_pitch_motor.gimbal_motor_mode == GIMBAL_MOTOR_RAW)
    {
        //raw控制
        gimbal_motor_raw_angle_control(&gimbal_control_loop->gimbal_pitch_motor);
    }
    else if (gimbal_control_loop->gimbal_pitch_motor.gimbal_motor_mode == GIMBAL_MOTOR_ENCONDE)
    {
        //enconde角度控制
//        gimbal_motor_relative_angle_control(&gimbal_control_loop->gimbal_pitch_motor);
		gimbal_motor_pixel_control(&gimbal_control_loop->gimbal_pitch_motor);
    }
}


static void gimbal_motor_pixel_control(Gimbal_Motor_t *gimbal_motor)
{
	if (gimbal_motor == NULL)
    {
        return;
    }
	
	//反馈值换为新数值
	gimbal_motor->current_set = PID_Calc(&gimbal_motor->gimbal_motor_pixel_pid, gimbal_motor->pixel_cmd, 0);
	gimbal_motor->given_current = (gimbal_motor->current_set);
}



static void gimbal_motor_relative_angle_control(Gimbal_Motor_t *gimbal_motor)
{
    if (gimbal_motor == NULL)
    {
        return;
    }

    //角度环，速度环串级pid调试
//	gimbal_motor->relative_angle_set = Yaw_Encoder_Sen*(gimbal_motor->raw_cmd_current);
	
//    gimbal_motor->motor_gyro_set = GIMBAL_PID_Calc(&gimbal_motor->gimbal_motor_relative_angle_pid, gimbal_motor->relative_angle, gimbal_motor->relative_angle_set, gimbal_motor->motor_gyro);
    gimbal_motor->current_set = PID_Calc(&gimbal_motor->gimbal_motor_relative_angle_pid, gimbal_motor->gimbal_motor_measure->real_one->relative_angle, gimbal_motor->relative_angle_set);
    //控制值赋值
    gimbal_motor->given_current = -(gimbal_motor->current_set);
}

static void gimbal_motor_raw_angle_control(Gimbal_Motor_t *gimbal_motor)
{
    if (gimbal_motor == NULL)
    {
        return;
    }
    gimbal_motor->current_set = gimbal_motor->raw_cmd_current;
    gimbal_motor->given_current =  -(gimbal_motor->current_set);
}

//返回云台控制变量，通过指针传递方式传递信息
const Gimbal_Control_t *get_gimbal_control_point(void)
{
    return &gimbal_control;
}
const Gimbal_Motor_t *get_yaw_motor_point(void)
{
    return &gimbal_control.gimbal_yaw_motor;
}
const Gimbal_Motor_t *get_pitch_motor_point(void)
{
    return &gimbal_control.gimbal_pitch_motor;
}

void gimbal_offset_init(void)
{
    gimbal_control.gimbal_yaw_motor.offset_ecd = 0;
    gimbal_control.gimbal_yaw_motor.max_relative_angle = PI/4;
    gimbal_control.gimbal_yaw_motor.min_relative_angle = -PI/4;

    gimbal_control.gimbal_pitch_motor.offset_ecd = 0;
    gimbal_control.gimbal_pitch_motor.max_relative_angle = 0.65;   
    gimbal_control.gimbal_pitch_motor.min_relative_angle = -0.45;
}

static void GIMBAL_PID_Init(Gimbal_PID_t *pid, fp32 maxout, fp32 max_iout, fp32 kp, fp32 ki, fp32 kd)
{
    if (pid == NULL)
    {
        return;
    }
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;

    pid->err = 0.0f;
    pid->get = 0.0f;

    pid->max_iout = max_iout;
    pid->max_out = maxout;
}
static fp32 GIMBAL_PID_Calc(Gimbal_PID_t *pid, fp32 get, fp32 set, fp32 error_delta)
{
    fp32 err;
    if (pid == NULL)
    {
        return 0.0f;
    }
    pid->get = get;
    pid->set = set;

    err = set - get;
    pid->err = rad_format(err);
    pid->Pout = pid->kp * pid->err;
    pid->Iout += pid->ki * pid->err;
    pid->Dout = pid->kd * error_delta;
    abs_limit(&pid->Iout, pid->max_iout);
    pid->out = pid->Pout + pid->Iout + pid->Dout;
    abs_limit(&pid->out, pid->max_out);
    return pid->out;
}

//pid数据清理
static void Gimbal_PID_clear(Gimbal_PID_t *gimbal_pid_clear)
{
    if (gimbal_pid_clear == NULL)
    {
        return;
    }
    gimbal_pid_clear->err = gimbal_pid_clear->set = gimbal_pid_clear->get = 0.0f;
    gimbal_pid_clear->out = gimbal_pid_clear->Pout = gimbal_pid_clear->Iout = gimbal_pid_clear->Dout = 0.0f;
}
